#ifndef CURVEFITTING_HPP
#define CURVEFITTING_HPP

#include "Function.hpp"
#include "Spline.hpp"
#include <vector>
#include <utility>
#include <cmath>

/**
 * @brief Define the Curve class
 */
class Curve {
public:
    int n; ///< Dimension
    Function* x; ///< x(t)
    Function* y; ///< y(t)
    double a, b; ///< Range of t

    /**
     * @brief Constructor
     * @param dim Dimension
     * @param xFunc Pointer to x(t) function
     * @param yFunc Pointer to y(t) function
     * @param t_min Minimum value of t
     * @param t_max Maximum value of t
     */
    Curve(int dim, Function* xFunc, Function* yFunc, double t_min, double t_max) 
        : n(dim), x(xFunc), y(yFunc), a(t_min), b(t_max) {}

    virtual ~Curve() = default;

    /**
     * @brief Calculate the total length of the curve
     * @param numSegments Number of segments
     * @return Total length of the curve
     */
    double totalLength(int numSegments = 1000) const {
        double length = 0.0;
        double dt = (b - a) / numSegments;
        for (int i = 0; i < numSegments; ++i) {
            double t1 = a + i * dt;
            double t2 = t1 + dt;
            double dx = x->operator()(t2) - x->operator()(t1);
            double dy = y->operator()(t2) - y->operator()(t1);
            length += std::sqrt(dx * dx + dy * dy);
        }
        return length;
    }

    /**
     * @brief Determine if the curve is self-intersecting
     * @param m Number of segments
     * @return Whether the curve is self-intersecting
     */
    bool isSelfIntersecting(int m) const {
        std::vector<double> t_values(m + 1);
        t_values[0] = a;
        double totalLength = 0.0;
        for (int i = 1; i <= m; ++i) {
            double t = a + i * (b - a) / m;
            double dx = x->operator()(t) - x->operator()(t - (b - a) / m);
            double dy = y->operator()(t) - y->operator()(t - (b - a) / m);
            totalLength += std::sqrt(dx * dx + dy * dy);
            t_values[i] = totalLength;
        }
        for (int i = 1; i <= m; ++i) {
            t_values[i] = a + (b - a) * (t_values[i] / totalLength);
        }

        for (int i = 1; i <= m; ++i) {
            double xi = x->operator()(t_values[i]);
            double yi = y->operator()(t_values[i]);
            for (int k = 0; k < i - 1; ++k) {
                double xk = x->operator()(t_values[k]);
                double yk = y->operator()(t_values[k]);
                double distance = std::abs(xi - xk) + std::abs(yi - yk);
                if (distance < 3 * (std::abs(x->operator()(t_values[i]) - x->operator()(t_values[i - 1])) + std::abs(y->operator()(t_values[i]) - y->operator()(t_values[i - 1])))) {
                    // Determine if the segments intersect
                    double x1 = x->operator()(t_values[i - 1]);
                    double y1 = y->operator()(t_values[i - 1]);
                    double x2 = x->operator()(t_values[i]);
                    double y2 = y->operator()(t_values[i]);
                    double x3 = x->operator()(t_values[k]);
                    double y3 = y->operator()(t_values[k]);
                    double x4 = x->operator()(t_values[k - 1]);
                    double y4 = y->operator()(t_values[k - 1]);

                    double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
                    if (std::abs(denominator) < 1e-9) {
                        continue; // Parallel or collinear
                    }

                    double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denominator;
                    double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denominator;

                    if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1) {
                        return true; // Segments intersect
                    }
                }
            }
        }
        return false;
    }
};

/**
 * @brief Define the PlaneCurve class
 */
class PlaneCurve : public Curve {
public:
    /**
     * @brief Constructor
     * @param xFunc Pointer to x(t) function
     * @param yFunc Pointer to y(t) function
     * @param t_min Minimum value of t
     * @param t_max Maximum value of t
     */
    PlaneCurve(Function* xFunc, Function* yFunc, double t_min, double t_max) 
        : Curve(2, xFunc, yFunc, t_min, t_max) {}
};

/**
 * @brief Calculate control points
 * @param curve Plane curve
 * @param m Number of segments
 * @param useChordalLength Whether to use chordal length parameterization
 * @return Vector of control points
 */
std::vector<std::vector<std::pair<double, double>>> calculateControlPoints(const PlaneCurve& curve, int m, bool useChordalLength) {
    std::vector<std::pair<double, double>> p;
    std::vector<std::pair<double, double>> tangent;
    std::vector<std::vector<std::pair<double, double>>> controlPoints;
    std::vector<double> t_values(m + 1);
    if (useChordalLength) {
        t_values[0] = curve.a;
        double totalLength = 0.0;
        for (int i = 1; i <= m; ++i) {
            double t = curve.a + i * (curve.b - curve.a) / m;
            double dx = curve.x->operator()(t) - curve.x->operator()(t - (curve.b - curve.a) / m);
            double dy = curve.y->operator()(t) - curve.y->operator()(t - (curve.b - curve.a) / m);
            totalLength += std::sqrt(dx * dx + dy * dy);
            t_values[i] = totalLength;
        }
        for (int i = 1; i <= m; ++i) {
            t_values[i] = curve.a + (curve.b - curve.a) * (t_values[i] / totalLength);
        }
    } else {
        for (int i = 0; i <= m; ++i) {
            t_values[i] = curve.a + i * (curve.b - curve.a) / m;
        }
    }

    for (double t : t_values) {
        p.push_back({curve.x->operator()(t), curve.y->operator()(t)});
        double dx = (curve.x->operator()(t + 1e-7) - curve.x->operator()(t - 1e-7)) / (2 * 1e-7);
        double dy = (curve.y->operator()(t + 1e-7) - curve.y->operator()(t - 1e-7)) / (2 * 1e-7);
        tangent.push_back({dx, dy});
    }

    for (int j = 0; j < m; ++j) {
        std::pair<double, double> q0 = p[j];
        std::pair<double, double> q1 = {q0.first + ((curve.b - curve.a) / 3.0 / m) * tangent[j].first, q0.second + ((curve.b - curve.a) / 3.0 / m) * tangent[j].second};
        std::pair<double, double> q2 = {p[j + 1].first - ((curve.b - curve.a) / 3.0 / m) * tangent[j + 1].first, p[j + 1].second - ((curve.b - curve.a) / 3.0 / m) * tangent[j + 1].second};
        std::pair<double, double> q3 = p[j + 1];

        controlPoints.push_back({q0, q1, q2, q3});
    }

    return controlPoints;
}

/**
 * @brief Define the PlaneCurveBezierFitting class for fitting PlaneCurve with Bezier curves
 */
class PlaneCurveBezierFitting {
public:
    /**
     * @brief Constructor
     * @param curve Plane curve
     * @param m Number of segments
     * @param useChordalLength Whether to use chordal length parameterization
     */
    PlaneCurveBezierFitting(const PlaneCurve& curve, int m, bool useChordalLength) 
        : curve(curve), m(m), controlPoints(calculateControlPoints(curve, m, useChordalLength)) {}

    /**
     * @brief Evaluate the Bezier curve at t
     * @param t Parameter t
     * @return Value of the Bezier curve at t
     */
    std::pair<double, double> evaluate(double t) const {
        int segment = std::min(static_cast<int>((t - curve.a) / ((curve.b - curve.a) / m)), m - 1);
        double local_t = (t - curve.a - segment * (curve.b - curve.a) / m) / ((curve.b - curve.a) / m);
        const auto& cp = controlPoints[segment];
        double x = std::pow(1 - local_t, 3) * cp[0].first + 3 * std::pow(1 - local_t, 2) * local_t * cp[1].first + 3 * (1 - local_t) * std::pow(local_t, 2) * cp[2].first + std::pow(local_t, 3) * cp[3].first;
        double y = std::pow(1 - local_t, 3) * cp[0].second + 3 * std::pow(1 - local_t, 2) * local_t * cp[1].second + 3 * (1 - local_t) * std::pow(local_t, 2) * cp[2].second + std::pow(local_t, 3) * cp[3].second;
        return {x, y};
    }

    /**
     * @brief Get the minimum value of t
     * @return Minimum value of t
     */
    double getTMin() const {
        return curve.a;
    }

    /**
     * @brief Get the maximum value of t
     * @return Maximum value of t
     */
    double getTMax() const {
        return curve.b;
    }

private:
    const PlaneCurve& curve;
    int m;
    std::vector<std::vector<std::pair<double, double>>> controlPoints;
};

/**
 * @brief Define the PlaneCurveCubicSplineFitting class for fitting PlaneCurve with natural cubic splines
 */
class PlaneCurveCubicSplineFitting {
public:
    /**
     * @brief Constructor
     * @param curve Plane curve
     * @param m Number of segments
     */
    PlaneCurveCubicSplineFitting(const PlaneCurve& curve, int m) 
        : curve(curve), m(m), xSpline(generateSpline(curve.x, m)), ySpline(generateSpline(curve.y, m)) {}

    /**
     * @brief Evaluate the spline curve at t
     * @param t Parameter t
     * @return Value of the spline curve at t
     */
    std::pair<double, double> evaluate(double t) const {
        double x = xSpline.evaluate(t);
        double y = ySpline.evaluate(t);
        return {x, y};
    }

    /**
     * @brief Get the minimum value of t
     * @return Minimum value of t
     */
    double getTMin() const {
        return curve.a;
    }

    /**
     * @brief Get the maximum value of t
     * @return Maximum value of t
     */
    double getTMax() const {
        return curve.b;
    }

private:
    const PlaneCurve& curve;
    int m;
    NaturalCubicPPForm xSpline;
    NaturalCubicPPForm ySpline;

    /**
     * @brief Generate the spline curve
     * @param func Pointer to the function
     * @param m Number of segments
     * @return Spline curve
     */
    NaturalCubicPPForm generateSpline(Function* func, int m) {
        std::vector<double> t_values(m + 1);
        for (int i = 0; i <= m; ++i) {
            t_values[i] = curve.a + i * (curve.b - curve.a) / m;
        }
        return NaturalCubicPPForm(t_values, *func);
    }
};

#endif // CURVEFITTING_HPP
